{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Report for aircraft-information-system-design\n",
    "* 姓名：李浩玮\n",
    "* 学号：2018300465\n",
    "\n",
    "## 内容：\n",
    "* 任务类型：路径规划\n",
    "\n",
    "* 背景介绍：目前无人机虽然实现了自动飞行，但是大部分的无人机仍不具备感知环境、动态适应环境的能力。本课题通过一步一步实现无人机的感知、路径规划、仿真，通过本项目理解无人机系统的感知与控制\n",
    "\n",
    "\n",
    "## 要求：\n",
    "\n",
    "1. 无人机从起点出发，朝向终点飞行，但是事先不知道环境地图（障碍物）的分布。\n",
    "\n",
    "2. 假设无人机上装备了激光雷达，能够探测一定范围内的障碍物信息\n",
    "\n",
    "   当前探测到的障碍物用红色显示\n",
    "\n",
    "   还未探测到的障碍物用灰色显示\n",
    "\n",
    "   已经探测到的障碍物用黑色显示\n",
    "\n",
    "3. 无人机利用A * 算法规划当前认为可行的路径，绿色的线段\n",
    "\n",
    "4. 但是随着探测范围扩大，无人机在飞行过程发现，利用之前规划的路线无法达到目标点，则需要重新规划路径\n",
    "\n",
    "5. 利用上述方法，不断探测障碍物，并不断规划路径，无人机最终到达目标点\n",
    "\n",
    "## 解决途径:\n",
    "### Step1 实现最基本的地图加载、显示\n",
    "地图文件为二进制文件，仿照老师所给的C++代码，用python实现数据的读取处理和可视化。\n",
    "地图文件的格式是一个二进制文件，文件的结构如下：\n",
    "* uint32_t (文件标识，内容是：0x15432345)\n",
    "* int32_t （map的宽度-mx，像素数）\n",
    "* int32_t （map的高度-my，像素数）\n",
    "* int8_t * mx * my (map数组，mx * my个int32_t 类型数据，具体的每个点的类型)\n",
    "* int32_t （起点的X坐标，startX）\n",
    "* int32_t （起点的Y坐标，startY）\n",
    "* int32_t （终点的X坐标，endX）\n",
    "* int32_t （终点的Y坐标，endY）\n",
    "定义好MAGIC ==np.uint32(0x15432345)，UINT_32_SIZE = 4，UINT_8_SIZE = 1 后循环读取就可以，最后作为一个二维数组，用sns.heatmap显示出地图形状，源代码如下："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "33 19 55 72\n"
     ]
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 2 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "43 20 40 83\n"
     ]
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 2 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "import numpy as np\n",
    "import seaborn as sns\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "#按照老师给的C++代码仿写读取\n",
    "#定义magic number\n",
    "MAGIC = np.uint32(0x15432345)\n",
    "UINT_32_SIZE = 4\n",
    "UINT_8_SIZE = 1\n",
    "\n",
    "def load_map(addr):\n",
    "    file = open(addr, \"rb\")\n",
    "    my_magic = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=False)\n",
    "    if my_magic != MAGIC:\n",
    "        return -1\n",
    "    mx = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)\n",
    "    my = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)\n",
    "    _map = [[] for i in range(mx)]\n",
    "    for i in range(0, mx * my):\n",
    "        k = int(i / mx)\n",
    "        _map[k].append(int.from_bytes(file.read(UINT_8_SIZE), byteorder='little', signed=True))\n",
    "    m_start_x = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)\n",
    "    m_start_y = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)\n",
    "    m_end_x = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)\n",
    "    m_end_y = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)\n",
    "    print(m_start_x,m_start_y,m_end_x,m_end_y) #显示图片，暂时用热力图看一下大概\n",
    "    file.close()\n",
    "    return _map\n",
    "\n",
    "fig = plt.figure()\n",
    "sns_plot = sns.heatmap(load_map(\"test_7.map\"), cmap=\"Greys\")\n",
    "plt.show()\n",
    "fig = plt.figure()\n",
    "sns_plot = sns.heatmap(load_map(\"test_1.map\"), cmap=\"Greys\")\n",
    "plt.show()\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "读取地图的函数输入二进制文件地址，返还一个二阶矩阵代表地图，0为可走，1为障碍物。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Step2 实现路径规划与显示\n",
    "在前面程序的基础上，实现A * 算法，在这个阶段可以假设环境地图都已知，规划全局的地图，并显示在GUI界面上。\n",
    "#### 2-1 A*算法\n",
    "A*算法是一种很常用的路径查找和图形遍历算法。它有较好的性能和准确度，是把广度优先（Breadth First）算法，Dijkstra算法，最佳优先搜索的特点集于一身的一种寻路算法。地图中每一个节点的优先级由f(n)=g(n)+h(n)求得，其中\n",
    "* f(n)是节点n的综合优先级。当我们选择下一个要遍历的节点时，我们总会选取综合优先级最高（值最小）的节点。\n",
    "* g(n) 是节点n距离起点的代价。\n",
    "* h(n)是节点n距离终点的预计代价，这也就是A*算法的启发函数。我在这里使用的是曼哈顿距离，同时只允许上下左右四个方向移动\n",
    "A*算法在运算过程中，每次从优先队列中选取f(n)值最小（优先级最高）的节点作为下一个待遍历的节点。另外，A*算法使用两个集合来表示待遍历的节点，与已经遍历过的节点，这通常称之为openset和closeset。\n",
    "完整的A*算法描述如下：\n",
    "\n",
    "* 初始化open_set和close_set；\n",
    "* 将起点加入open_set中，并设置优先级为0（优先级最高）；\n",
    "* 如果open_set不为空，则从open_set中选取优先级最高的节点n：\n",
    "    * 如果节点n为终点，则：\n",
    "        * 从终点开始逐步追踪parent节点，一直达到起点；\n",
    "        * 返回找到的结果路径，算法结束；\n",
    "    * 如果节点n不是终点，则：\n",
    "        * 将节点n从open_set中删除，并加入close_set中；\n",
    "        * 遍历节点n所有的邻近节点：\n",
    "            * 如果邻近节点m在close_set中，则：\n",
    "                * 跳过，选取下一个邻近节点\n",
    "            * 如果邻近节点m也不在open_set中，则：\n",
    "                * 设置节点m的parent为节点n\n",
    "                * 计算节点m的优先级\n",
    "                * 将节点m加入open_set中\n",
    "                \n",
    "#### 2-2 源代码以及详细注释以及结果分析\n",
    "源代码以及详细注释如下："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# -*- coding: utf-8 -*-\n",
    "\"\"\"\n",
    "Created on Mon Jan  3 16:20:14 2022\n",
    "@author: saw\n",
    "重构代码时部分借鉴了\n",
    "1.https://www.jianshu.com/p/613ef51394ec\n",
    "2.https://blog.csdn.net/qq_39687901/article/details/80753433\n",
    "中的代码例子对我的进行完善\n",
    "\"\"\"\n",
    "\n",
    "\"\"\"\n",
    "算法伪代码：\n",
    "1.把起点加入 open list 。\n",
    "2.重复如下过程：\n",
    "a.遍历 open list ，查找 F 值最小的节点，把它作为当前要处理的节点。\n",
    "b.把这个节点移到 close list 。\n",
    "c.对当前方格的 8 个相邻方格的每一个方格？            \n",
    "    如果它是不可抵达的或者它在 close list 中，忽略它。否则，做如下操作。\n",
    "    如果它不在 open list 中，把它加入 open list ，并且把当前方格设置为它的父亲，记录该方格的 F ， G 和 H 值。\n",
    "    如果它已经在 open list 中，检查这条路径 ( 即经由当前方格到达它那里 ) 是否更好，用 G 值作参考。更小的 G 值表示这是更好的路径。如果是这样，把它的父亲设置为当前方格，并重新计算它                                                                的 G 和 F 值。如果你的 open list 是按 F 值排序的话，改变后你可能需要重新排序。\n",
    "d.停止，当你\n",
    "    把终点加入到了 open list 中，此时路径已经找到了，或者\n",
    "    查找终点失败，并且 open list 是空的，此时没有路径。\n",
    "3.保存路径。从终点开始，每个方格沿着父节点移动直至起点，这就是你的路径。\n",
    "\"\"\"\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from matplotlib import pyplot as plt\n",
    "from matplotlib import colors\n",
    "\n",
    "#仿照老师给的C++代码读取地图文件\n",
    "MAGIC = np.uint32(0x15432345)\n",
    "UINT_32_SIZE = 4\n",
    "UINT_8_SIZE = 1\n",
    "def load_map(addr):\n",
    "    file = open(addr, \"rb\")\n",
    "    my_magic = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=False)\n",
    "    if my_magic != MAGIC:\n",
    "        return -1\n",
    "    mx = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)\n",
    "    my = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)\n",
    "    _map = [[] for i in range(mx)]\n",
    "    for i in range(0, mx * my):\n",
    "        k = int(i / mx)\n",
    "        _map[k].append(int.from_bytes(file.read(UINT_8_SIZE), byteorder='little', signed=True))\n",
    "    m_start_x = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)\n",
    "    m_start_y = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)\n",
    "    m_end_x = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)\n",
    "    m_end_y = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)\n",
    "\n",
    "    file.close()\n",
    "    return _map\n",
    "\n",
    "#定义地图类 包括高h，宽w，数据data\n",
    "class Maplist:\n",
    "    def __init__(self,maplist):\n",
    "        self.w=np.shape(maplist)[1]\n",
    "        self.h=np.shape(maplist)[0]\n",
    "        self.data=[]\n",
    "        self.data=maplist\n",
    "        \n",
    "        #绘图主要用pcolor函数，起点，终点，障碍，路径分别对应不同的颜色\n",
    "    def show_obstructBE(self,startpoint,endpoint):  #绘制当前地图\n",
    "        self.data[startpoint.x][startpoint.y]=2\n",
    "        self.data[endpoint.x][endpoint.y]=3\n",
    "        cmap = colors.ListedColormap(['white','black','red','blue'])\n",
    "        plt.figure(figsize=(6,6))\n",
    "        plt.pcolor(self.data[::-1],cmap=cmap,linewidths=1)\n",
    "        plt.axis('equal') \n",
    "        plt.axis('off') \n",
    "        plt.show()\n",
    "    \n",
    "    def show_path(self,point,i):\n",
    "        self.data[point.x][point.y]=4\n",
    "        cmap = colors.ListedColormap(['white','black','red','blue','green'])\n",
    "        plt.figure(figsize=(6,6))\n",
    "        plt.pcolor(self.data[::-1],cmap=cmap,linewidths=1)\n",
    "        plt.axis('equal') \n",
    "        plt.axis('off') \n",
    "        #保存每帧图片，之后做成gif\n",
    "        path='step2image_demo/'\n",
    "        plt.savefig(path + str(i+1)+'.png')\n",
    "        plt.show()\n",
    "\n",
    "    def __getitem__(self, item):\n",
    "        return self.data[item]\n",
    "   \n",
    "\n",
    "#定义点类，包含点的横纵坐标\n",
    "class Point:\n",
    "    def __init__(self,x,y):\n",
    "        self.x=x;self.y=y \n",
    "    def __eq__(self, other):\n",
    "        if self.x==other.x and self.y==other.y:\n",
    "            return True\n",
    "        return False\n",
    "\n",
    "#定义A*算法主体，包括各函数，主循环\n",
    "class AStar: \n",
    "    # 定义节点 可以完美和点区分开（point和node两类）\n",
    "    class Node:  # 描述AStar算法中的节点数据\n",
    "        def __init__(self, point, endPoint, g=0):\n",
    "            self.point = point  # 当前节点的坐标\n",
    "            self.parent = None  # 上一级节点\n",
    "            self.g = g  # g()值\n",
    "            # h()值,两坐标轴之差的绝对值的和\n",
    "            self.h = (abs(endPoint.x - point.x) + abs(endPoint.y - point.y))   \n",
    " \n",
    "    def __init__(self, nowmap, startPoint, endPoint,  obstruct=1):\n",
    "        # 创建openlist和closelist\n",
    "        self.openList = []\n",
    "        self.closeList = []\n",
    "        self.nowmap = nowmap  #导入地图\n",
    "        #Astar中起点和终点\n",
    "        self.startPoint = startPoint  \n",
    "        self.endPoint = endPoint\n",
    "        #定义地图上的障碍为obstrust=1\n",
    "        self. obstruct =  obstruct\n",
    "\n",
    "    #判断节点是否在closelist中\n",
    "    def IsInCloseList(self, point):\n",
    "        for node in self.closeList:\n",
    "            if node.point == point:\n",
    "                return True\n",
    "        return False\n",
    "    \n",
    "    #判断节点是否在openlist中\n",
    "    def IsInOpenList(self, point):\n",
    "        for node in self.openList:\n",
    "            if node.point == point:\n",
    "                return node\n",
    "        return None\n",
    "\n",
    "    #找openlist中F最小的节点\n",
    "    def getMinNode(self):\n",
    "        minNode = self.openList[0]\n",
    "        for node in self.openList:\n",
    "            if node.g + node.h < minNode.g + minNode.h:\n",
    "                minNode = node\n",
    "        return minNode\n",
    "\n",
    "    #判断终点是否在openlist中，作为最终结果依据：\n",
    "    #把终点加入到了 open list 中，此时路径已经找到了，\n",
    "    #或者查找终点失败，并且 open list 是空的，此时没有路径。\n",
    "    def IsendPointInopenList(self):\n",
    "        for node in self.openList:\n",
    "            if node.point == self.endPoint:\n",
    "                return node\n",
    "        return None\n",
    "\n",
    "    #计算当前点四周的节点数据（不允许斜着走）\n",
    "    def Changenode(self, minFnode, dertaX, dertaY):\n",
    "        # 当目标点碰到边界或是障碍或是在closelist中，不做考虑直接跳过\n",
    "        if minFnode.point.x + dertaX < 0 or minFnode.point.x + dertaX > self.nowmap.w - 1 or minFnode.point.y + dertaY < 0 or minFnode.point.y + dertaY > self.nowmap.h - 1:\n",
    "            return\n",
    "        if self.nowmap[minFnode.point.x + dertaX][minFnode.point.y + dertaY] == self.obstruct:\n",
    "            return\n",
    "        nowPoint = Point(minFnode.point.x + dertaX, minFnode.point.y + dertaY)\n",
    "        if self.IsInCloseList(nowPoint):\n",
    "            return\n",
    "                # 设置g()的变量(每次移动加1)\n",
    "        derta_g = 1\n",
    "        # 如果不再openList中，就把它加入openlist，并且把当前方格设置为它的上级节点，记录该方格的 F()，G() 和H()值。\n",
    "        newNode = self.IsInOpenList(nowPoint)\n",
    "        if not newNode:\n",
    "            newNode = AStar.Node(nowPoint, self.endPoint, g=minFnode.g + derta_g)\n",
    "            newNode.parent = minFnode\n",
    "            self.openList.append(newNode)\n",
    "            return\n",
    "        \n",
    "        # 如果在openList中，判断minFnode到当前点的G是否更小\n",
    "        if minFnode.g + derta_g < newNode.g:  # 如果更小，就重新计算g值，并且改变上一级节点\n",
    "            newNode.g = minFnode.g + derta_g\n",
    "            newNode.parent = minFnode\n",
    " \n",
    "    #算法主逻辑循环\n",
    "    def start(self):\n",
    "        # 1.将起点放入openlist\n",
    "        startNode = AStar.Node(self.startPoint, self.endPoint)\n",
    "        self.openList.append(startNode)\n",
    "        # 2.主循环逻辑\n",
    "        while True:\n",
    "            # 遍历 openlist ，查找 F()值最小的节点，把它作为当前要处理的节点\n",
    "            minFnode = self.getMinNode()\n",
    "            # 把这个点加入closeList中，并且在openList中删除它\n",
    "            self.closeList.append(minFnode)\n",
    "            self.openList.remove(minFnode)\n",
    "            # 判断这个节点的上下左右节点，这里的逻辑在Changenode()函数中已经写过\n",
    "            self.Changenode(minFnode, 0, -1)\n",
    "            self.Changenode(minFnode, 0, 1)\n",
    "            self.Changenode(minFnode, -1, 0)\n",
    "            self.Changenode(minFnode, 1, 0)\n",
    "            # 判断是否终止\n",
    "            resultpoint = self.IsendPointInopenList()\n",
    "            if resultpoint != None:  # 如果终点在openlist中，则找到路径。\n",
    "                finalPoint = resultpoint\n",
    "                #创建pathlist，里面存放路径上的节点\n",
    "                pathList = []\n",
    "                #循环，倒推路径\n",
    "                while 1:\n",
    "                    if finalPoint.parent != None:\n",
    "                        pathList.append(finalPoint.point)\n",
    "                        finalPoint=finalPoint.parent\n",
    "                    #找到起点，把路线反向传出，结束。    \n",
    "                    if finalPoint.parent == None:                        \n",
    "                        return list(reversed(pathList))\n",
    "\n",
    "            # openlist是空的，此时没有路径        \n",
    "            if len(self.openList) == 0:\n",
    "                return None\n",
    "            \n",
    "if __name__ == '__main__':\n",
    "    #读地图文件\n",
    "    nowmap=Maplist(load_map(\"test_7.map\"))\n",
    "    #显示初始地图，包括障碍，起点，终点\n",
    "    startpoint=Point(20, 43)\n",
    "    endPoint=Point(83, 40)\n",
    "    nowmap.show_obstructBE(startpoint,endPoint)\n",
    "    #设置起点终点，寻路\n",
    "    aStar=AStar(nowmap,startpoint,endPoint)\n",
    "    #描绘路径，显示有路径的地图\n",
    "    pathList=aStar.start()\n",
    "   #i作为每帧图片名字\n",
    "    i=0\n",
    "    for point in pathList:\n",
    "        ax=nowmap.show_path(point,i)\n",
    "        i+=1"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "1 结果如下(程序结果中最后一张图)：\n",
    "\n",
    "![map1](images/97.png)\n",
    "![map2](images/118.png)\n",
    "\n",
    "黑色为障碍，较浅的为Astar找到的路径，整体路线基本是最优。这里我采用了曼哈顿距离，同时只允许上下左右四个方向移动，因为一开始是写的八个方向都可以移动，发现路线会从图形的倾斜的边缘穿过去，感觉这样和实际情况不符合，就改为了四个方向移动。\n",
    "\n",
    "2 对于h(n)函数的选取：\n",
    "* 当启发函数h(n)始终为0，则将由g(n)决定节点的优先级，此时算法就退化成了Dijkstra算法。\n",
    "* 如果h()n相较于g(n)大很多，则此时只有h(n)产生效果，这也就变成了最佳优先搜索。\n",
    "* 如果h(n)始终小于等于节点n到终点的代价，则A*算法保证一定能够找到最短路径。但是当h(n)的值越小，算法将遍历越多的节点，也就导致算法越慢。\n",
    "* 如果h(n)完全等于节点n到终点的代价，则A*算法将找到最佳路径，并且速度很快。可惜的是，并非所有场景下都能做到这一点。因为在没有达到终点之前，我们很难确切算出距离终点还有多远。\n",
    "* 如果h(n)的值比节点n到终点的代价要大，则A*算法不能保证找到最短路径，不过此时会很快。\n",
    "\n",
    "所以可以根据需求灵活选择。\n",
    "\n",
    "3 结果的可视化一开始只是用heatmap热力图画了一下结果。之后用pcolor()重新进行了可视化，红色为起点，蓝色为终点，绿色是路径，每走一步生成一帧，就可以实现动图。  \n",
    "\n",
    "4 自己写完代码后看了许多前辈的代码，花了大力气重构了一下，最大的改动就是把点分两个class：point和node，这样过程就清晰了很多，python的优势也体现了出来，不需要花费心思来处理宽度很大的需要同时记录点坐标，上级点坐标的数列。\n",
    "\n",
    "最终结果做成gif效果如下：\n",
    "\n",
    "![result1](images/Astar.gif)\n",
    "![result2](images/Astar2.gif)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Step3 实现激光雷达的扫描\n",
    "#### 3.1 问题分析\n",
    "雷达向四周发射射线后通过反射判断障碍物，首先，雷达有一定的范围，范围外的障碍物不可知，其次，因为射线遇到障碍物后会反射而不会穿过去，所以障碍物后面的情况不可知。这里做了一个模型的简化：因为地图上的障碍是单层的，所以让雷达可以知道探测半径范围内的所有情况对结果没有影响。\n",
    "#### 3.2 编程思路\n",
    "初始雷达的地图全为可走区域，每走一步后进行探测，对自己的地图进行更新，并传给Astar进行路线规划，探测具体步骤：以飞行器目前为止为终点画边长为二倍雷达探测半径的正方形（保证不出边界），在这个范围内进行枚举，计算离中心点的距离，去掉距离超出探测半径的点，剩下的对雷达地图进行覆盖。\n",
    "代码以及详细注释如下："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "# -*- coding: utf-8 -*-\n",
    "\"\"\"\n",
    "Created on Tue Jan  4 15:19:47 2022\n",
    "\n",
    "@author: saw\n",
    "\"\"\"\n",
    "import numpy as np\n",
    "class Radar:\n",
    "    #雷达扫描半径，初始时认为处处无障碍，每次移动刷新地图   \n",
    "    def __init__(self,radar_R,realmap):\n",
    "        self.w=np.shape(realmap)[1]\n",
    "        self.h=np.shape(realmap)[0]\n",
    "        self.R=radar_R\n",
    "        knowmap=np.zeros([self.w,self.h])\n",
    "    #判断点是否在雷达范围内\n",
    "    def instance(self,a,b,c,d):\n",
    "        if (a-c)*(a-c)+(b-d)*(b-d)<self.R*self.R:\n",
    "            return True\n",
    "        \n",
    "    #雷达扫描，扫描的点不出边界，不出雷达半径，返回更新后的地图用于Astar    \n",
    "    def searchfun(self,point,Maplist,Maplist_):\n",
    "        knowmap=Maplist_.data\n",
    "        i_min=max(point.x-self.R,0)\n",
    "        i_max=min(point.x+self.R,self.w)\n",
    "        j_min=max(point.y-self.R,0)\n",
    "        j_max=min(point.y+self.R,self.h)\n",
    "        for i in range(i_min,i_max):\n",
    "            for j in range(j_min,j_max):\n",
    "                if self.instance(i,j,point.x,point.y):\n",
    "                    knowmap[i][j]=Maplist.data[i][j]\n",
    "        return knowmap"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Step4 动态路径规划\n",
    "#### 4.1 问题分析与简化 \n",
    "Astar算法已经实现，雷达的功能也已经实现，只需要调用前面的代码，在主程序里运用即可。初步思路：飞行器按照雷达函数更新地图，然后运用Astar在已知地图上规划路线，按照路线每前进一步更新一次地图，每更新一次地图把当前位置作为新的起点更新一次路径，循环一直到终点。\n",
    "\n",
    "#### 4.2 源代码以及详细注释以及结果分析\n",
    "源代码及详细注释如下："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# -*- coding: utf-8 -*-\n",
    "\"\"\"\n",
    "Created on Tue Jan  4 17:07:42 2022\n",
    "\n",
    "@author: saw\n",
    "\"\"\"\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from matplotlib import colors\n",
    "\n",
    "\n",
    "#地图载入\n",
    "MAGIC = np.uint32(0x15432345)\n",
    "UINT_32_SIZE = 4\n",
    "UINT_8_SIZE = 1\n",
    "def load_map(addr):\n",
    "    file = open(addr, \"rb\")\n",
    "    my_magic = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=False)\n",
    "    if my_magic != MAGIC:\n",
    "        return -1\n",
    "    mx = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)\n",
    "    my = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)\n",
    "    _map = [[] for i in range(mx)]\n",
    "    for i in range(0, mx * my):\n",
    "        k = int(i / mx)\n",
    "        _map[k].append(int.from_bytes(file.read(UINT_8_SIZE), byteorder='little', signed=True))\n",
    "    m_start_x = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)\n",
    "    m_start_y = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)\n",
    "    m_end_x = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)\n",
    "    m_end_y = int.from_bytes(file.read(UINT_32_SIZE), byteorder='little', signed=True)\n",
    "\n",
    "    file.close()\n",
    "    return _map\n",
    "\n",
    " #定义点类，包含点的横纵坐标\n",
    "class Point:\n",
    "    def __init__(self,x,y):\n",
    "        self.x=x;self.y=y \n",
    "    def __eq__(self, other):\n",
    "        if self.x==other.x and self.y==other.y:\n",
    "            return True\n",
    "        return False\n",
    "\n",
    "class Radar:\n",
    "    \n",
    "    def __init__(self,radar_R,realmap):\n",
    "        self.w=np.shape(realmap)[1]\n",
    "        self.h=np.shape(realmap)[0]\n",
    "        self.R=radar_R\n",
    "        knowmap=np.zeros([self.w,self.h])\n",
    "    \n",
    "    def instance(self,a,b,c,d):\n",
    "        if (a-c)*(a-c)+(b-d)*(b-d)<self.R*self.R:\n",
    "            return True\n",
    "        \n",
    "        \n",
    "    def searchfun(self,point,Maplist,Maplist_):\n",
    "        knowmap=Maplist_.data\n",
    "        i_min=max(point.x-self.R,0)\n",
    "        i_max=min(point.x+self.R,self.w)\n",
    "        j_min=max(point.y-self.R,0)\n",
    "        j_max=min(point.y+self.R,self.h)\n",
    "        for i in range(i_min,i_max):\n",
    "            for j in range(j_min,j_max):\n",
    "                if self.instance(i,j,point.x,point.y):\n",
    "                    knowmap[i][j]=Maplist.data[i][j]\n",
    "        return knowmap\n",
    "#定义A*算法主体，包括各函数，主循环\n",
    "class AStar: \n",
    "    # 定义节点 可以完美和点区分开（point和node两类）\n",
    "    class Node:  # 描述AStar算法中的节点数据\n",
    "        def __init__(self, point, endPoint, g=0):\n",
    "            self.point = point  # 当前节点的坐标\n",
    "            self.parent = None  # 上一级节点\n",
    "            self.g = g  # g()值\n",
    "            # h()值,两坐标轴之差的绝对值的和\n",
    "            self.h = (abs(endPoint.x - point.x) + abs(endPoint.y - point.y))\n",
    "    def __init__(self, nowmap, startPoint, endPoint,  obstruct=1):\n",
    "        # 创建openlist和closelist\n",
    "        self.openList = []\n",
    "        self.closeList = []\n",
    "        self.nowmap = nowmap  #导入地图\n",
    "        #Astar中起点和终点\n",
    "        self.startPoint = startPoint  \n",
    "        self.endPoint = endPoint\n",
    "        #定义地图上的障碍为obstrust=1\n",
    "        self. obstruct =  obstruct\n",
    "    #判断节点是否在closelist中\n",
    "    def IsInCloseList(self, point):\n",
    "        for node in self.closeList:\n",
    "            if node.point == point:\n",
    "                return True\n",
    "        return False\n",
    "        #判断节点是否在openlist中\n",
    "    def IsInOpenList(self, point):\n",
    "        for node in self.openList:\n",
    "            if node.point == point:\n",
    "                return node\n",
    "        return None\n",
    "    #找openlist中F最小的节点\n",
    "    def getMinNode(self):\n",
    "        minNode = self.openList[0]\n",
    "        for node in self.openList:\n",
    "            if node.g + node.h < minNode.g + minNode.h:\n",
    "                minNode = node\n",
    "        return minNode\n",
    "    #判断终点是否在openlist中，作为最终结果依据：\n",
    "    #把终点加入到了 open list 中，此时路径已经找到了，\n",
    "    #或者查找终点失败，并且 open list 是空的，此时没有路径。\n",
    "    def IsendPointInopenList(self):\n",
    "        for node in self.openList:\n",
    "            if node.point == self.endPoint:\n",
    "                return node\n",
    "        return None\n",
    "    #计算当前点四周的节点数据（不允许斜着走）\n",
    "    def Changenode(self, minFnode, dertaX, dertaY):\n",
    "        # 当目标点碰到边界或是障碍或是在closelist中，不做考虑直接跳过\n",
    "        if minFnode.point.x + dertaX < 0 or minFnode.point.x + dertaX > self.nowmap.w - 1 or minFnode.point.y + dertaY < 0 or minFnode.point.y + dertaY > self.nowmap.h - 1:\n",
    "            return\n",
    "        if self.nowmap[minFnode.point.x + dertaX][minFnode.point.y + dertaY] == self.obstruct:\n",
    "            return\n",
    "        nowPoint = Point(minFnode.point.x + dertaX, minFnode.point.y + dertaY)\n",
    "        if self.IsInCloseList(nowPoint):\n",
    "            return\n",
    "                # 设置g()的变量(每次移动加1)\n",
    "        derta_g = 1\n",
    "        # 如果不再openList中，就把它加入openlist，并且把当前方格设置为它的上级节点，记录该方格的 F()，G() 和H()值。\n",
    "        newNode = self.IsInOpenList(nowPoint)\n",
    "        if not newNode:\n",
    "            newNode = AStar.Node(nowPoint, self.endPoint, g=minFnode.g + derta_g)\n",
    "            newNode.parent = minFnode\n",
    "            self.openList.append(newNode)\n",
    "            return\n",
    "                # 如果在openList中，判断minFnode到当前点的G是否更小\n",
    "        if minFnode.g + derta_g < newNode.g:  # 如果更小，就重新计算g值，并且改变上一级节点\n",
    "            newNode.g = minFnode.g + derta_g\n",
    "            newNode.parent = minFnode\n",
    "     #算法主逻辑循环\n",
    "    def start(self):\n",
    "        # 1.将起点放入openlist\n",
    "        startNode = AStar.Node(self.startPoint, self.endPoint)\n",
    "        self.openList.append(startNode)\n",
    "        # 2.主循环逻辑\n",
    "        while True:\n",
    "            # 遍历 openlist ，查找 F()值最小的节点，把它作为当前要处理的节点\n",
    "            minFnode = self.getMinNode()\n",
    "            # 把这个点加入closeList中，并且在openList中删除它\n",
    "            self.closeList.append(minFnode)\n",
    "            self.openList.remove(minFnode)\n",
    "            # 判断这个节点的上下左右节点，这里的逻辑在Changenode()函数中已经写过\n",
    "            self.Changenode(minFnode, 0, -1)\n",
    "            self.Changenode(minFnode, 0, 1)\n",
    "            self.Changenode(minFnode, -1, 0)\n",
    "            self.Changenode(minFnode, 1, 0)\n",
    "            # 判断是否终止\n",
    "            resultpoint = self.IsendPointInopenList()\n",
    "            if resultpoint != None:  # 如果终点在openlist中，则找到路径。\n",
    "                finalPoint = resultpoint\n",
    "                #创建pathlist，里面存放路径上的节点\n",
    "                pathList = []\n",
    "                #循环，倒推路径\n",
    "                while 1:\n",
    "                    if finalPoint.parent != None:\n",
    "                        pathList.append(finalPoint.point)\n",
    "                        finalPoint=finalPoint.parent\n",
    "                    #找到起点，把路线反向传出，结束。    \n",
    "                    if finalPoint.parent == None:                        \n",
    "                        return list(reversed(pathList))\n",
    "            # openlist是空的，此时没有路径        \n",
    "            if len(self.openList) == 0:\n",
    "                return None\n",
    "\n",
    "\n",
    "#以上为地图的载入，算法主体，雷达，与step2，3完全相同，引用即可\n",
    "            \n",
    "#定义地图类 包括高h，宽w，数据data\n",
    "class Maplist:\n",
    "    def __init__(self,maplist):\n",
    "        self.w=np.shape(maplist)[1]\n",
    "        self.h=np.shape(maplist)[0]\n",
    "        self.data=[]\n",
    "        self.data=maplist\n",
    "    \n",
    "    #更新了地图的画图方法    \n",
    "    # 0：可走，1：本来障碍，2：已经探测障碍，4：规划路线 5：起点，6：终点\n",
    "    def show_obstructBE(self,realmaplist,startpoint,endpoint,pathList,i):  #绘制当前地图\n",
    "        #标志起点终点\n",
    "        self.data[startpoint.x][startpoint.y]=2\n",
    "        self.data[endpoint.x][endpoint.y]=3\n",
    "        #标志目标路线\n",
    "        for point in pathList:\n",
    "            self.data[point.x][point.y]=4\n",
    "        cmap = colors.ListedColormap(['white','black','red','blue','green'])\n",
    "        #画图保存\n",
    "        plt.figure(figsize=(6,6))\n",
    "        plt.pcolor(self.data[::-1],cmap=cmap,linewidths=1)\n",
    "        plt.axis('equal') \n",
    "        plt.axis('off') \n",
    "        path='step4image_demo/'\n",
    "        plt.savefig(path + str(i+1)+'.png')\n",
    "        plt.show()\n",
    "    def __getitem__(self, item):\n",
    "        return self.data[item]\n",
    " \n",
    "if __name__ == '__main__':\n",
    "    #读地图文件\n",
    "    realmaplist=load_map(\"test_1.map\")\n",
    "    #定义地图，点\n",
    "    realmap=Maplist(realmaplist)\n",
    "    startpoint=Point(20, 43)\n",
    "    endPoint=Point(83, 40)\n",
    "    radar=Radar(25,realmaplist)\n",
    "    newmap=Maplist(np.zeros([realmap.w,realmap.h]))\n",
    "    #记录每张图片作为动图\n",
    "    picture_number=0\n",
    "    #没到终点一直循环\n",
    "    while startpoint.x!=endPoint.x or startpoint.y != endPoint.y:\n",
    "        #雷达更新地图\n",
    "        knowmap=Radar.searchfun(radar,startpoint,realmap,newmap) #list\n",
    "        newmap=Maplist(knowmap)\n",
    "        #规划路线\n",
    "        aStar=AStar(newmap,startpoint,endPoint)\n",
    "        pathList=aStar.start()\n",
    "        #过程可视化并保存\n",
    "        #newmap.show_obstructBE(realmaplist,startpoint, endPoint,pathList,picture_number)\n",
    "        drawmap.show_picture(startpoint, pathList)\n",
    "        #下一步变起点\n",
    "        startpoint=pathList[0]\n",
    "        picture_number+=1\n",
    "        "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "1.飞行器探测到的障碍物为黑色，没探测到则认为是可以走的路径，当前规划路线为绿色实线，红点为当前位置（每步起点）。结果部分截图如下：\n",
    "\n",
    "![step4_result1](images/step4_1.png)\n",
    "![step4_result2](images/step4_2.png) \n",
    "\n",
    "可以看到开始地图几乎全是未知的，飞行器规划直接到终点，随着路线前进，探测到“U”型障碍穿不过去后按已知重新规划，最终到达终点，此时地图基本探完。\n",
    "\n",
    "2.编写的程序是一步一规划，虽然每次都是Astar算法，一定可以找到终点，但路径不是最优。同时雷达模型的简化使得探索地图比实际要快速。\n",
    "\n",
    "3.因为花了很长的时间写Astar和雷达，所以模块化做的比较好，做最后一部分动态规划比我想象的好做不少。但依然有缺陷，对最后gif的制作，我用了笨办法：每一步画一张图存到本地，最后用图片做动图，这样每步都要重画一整副图，占用了大量空间同时程序也拖慢了程序的速度。\n",
    "\n",
    "最终动态规划gif结果如下：\n",
    "\n",
    "![step4_gif](images/Radar_Astar.gif)\n",
    "![step4_gif2](images/Radar_Astar2.gif)\n",
    "\n",
    "### step5 代码重构\n",
    "\n",
    "编写过程中，尽量对代码进行模块化，让整个程序主体由清晰的类以及自定义函数构成，主函数只占很少一部分，"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3. 深入思考\n",
    "* 如何实现无人机的定位？\n",
    "目前想法：1.以自己的初始为坐标原点，记录障碍等的坐标形成一个相对坐标系，如果知道变换矩阵，可以和绝对坐标进行转换；2，用GPS等硬件设备获得自己的位置信息，或者间隔时间进行位置矫正。\n",
    "* 如何使用三维激光雷达生成三维地图？\n",
    "目前想法 ：雷达射出射线，接受反射，可以知道障碍与自己的角度和距离，坐标变换到直角坐标系再生成地图。\n",
    "* 如何使用相机生成三维地图？\n",
    "目前想法：对图片进行深度学习训练神经网络，使得计算机对图中物体进行识别，定位等。（特斯拉）\n",
    "* 如何考虑飞行器的飞行特性，实现平滑的轨迹？\n",
    "* 如果是多个无人机，如何共享地图，加快搜索和任务执行效率？\n",
    "\n",
    "## 4.总结与收获\n",
    "1. 了解了二进制文件构成，学会了Astar算法。在编写Astar算法和雷达模型时尽量进行模块化，使得后面的工作额外的顺利。\n",
    "2. 开始准备用QT完成报告，但因为时间原因没有掌握这个工具，最终用自己比较熟悉的python完成了整个报告，但我在了解QT时看到了 qt for python 这个功能，感觉很有意思，日后也许可以自学一下。\n",
    "3. 自己的程序依然有许多不足：命名不太规范；类下的函数输入和返回设计的不太好，调用时总要改写；实现Astar算法时考虑了后面动态规划，但没考虑可视化；对于python有许多用法知其然而不知其所以然，日后要补补课。\n",
    "\n",
    "## 5.参考资料\n",
    "* (https://zhuanlan.zhihu.com/p/54510444)\n",
    "* (https://blog.csdn.net/Zhouzi_heng/article/details/115035298)\n",
    "* (https://blog.csdn.net/dujuancao11/article/details/109749219)\n",
    "* (https://www.zhihu.com/question/58959787/answer/1451873823?utm_source=qq&utm_medium=social&utm_oi=994631478822723584)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
